#include <ros/ros.h>
#include <tf/transform_datatypes.h>
#include <geometry_msgs/Twist.h>
#include <agv_msgs/OdometryLite.h>
#include <agv_msgs/PoseWithConfidence.h>
#include <agv_msgs/V_order.h>
#include <agv_msgs/V_node.h>
#include <agv_msgs/AgvAutoStatus.h>
#include <agv_msgs/AGVMode.h>
#include <agv_srvs/LidarCalib.h>
#include <agv_srvs/V_order.h>
#include <agv_srvs/SetOperationMode.h>
#include <agv_srvs/V_localOrder.h>
#include <agv_srvs/SetLaserCalib.h>
#include <std_msgs/String.h>
#include <std_msgs/Int32.h>
#include <boost/thread.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf/transform_listener.h>
#include <tf2_ros/transform_listener.h>
#include <Eigen/Dense>
#include <map_reader.h>
#include <numeric>
#include <thread>
#include <chrono>
#include <fstream>
#include <iostream>

#define PACKAGE_NAME "lidar_calibration"

using namespace std;

struct PoseRecord
{
    double x_;
    double y_;
    double theta_;
};


class AutoCalib
{
public:
    AutoCalib(ros::NodeHandle& nh):nh_(nh), percent(0){}
    ~AutoCalib(){
        //th_end();
        file1.close();
        file2.close();
        work_thread->interrupt();
        work_thread->join();
        work_thread.reset();
    }

    bool init();
    bool line_move(int fromNode,int toNode);
    double getdistance(geometry_msgs::Pose cur, geometry_msgs::Pose start){
        // ROS_INFO("cur_x:%.4f, cur_y:%.4f, start_x:%.4f, start_y:%.4f", cur.position.x, cur.position.y, start.position.x, start.position.y);
        return sqrt(pow((cur.position.x - start.position.x),2) + pow((cur.position.y - start.position.y),2));
    }
    void landmark_callback(const geometry_msgs::PoseStamped &mark_data);
    void HandleMoveInfoMessage(const agv_msgs::AgvAutoStatus::ConstPtr& msg);
    void InitMap(const ros::WallTimerEvent &timer_event);
    void PublishPercent(const ros::WallTimerEvent &timer_event);
    void RecordPose(const ros::WallTimerEvent &timer_event);
    void HandleModeInfoMessage(const agv_msgs::AGVMode::ConstPtr& msg);
    void pose_callback(const::agv_msgs::PoseWithConfidence& msg);
    void work_cycle();
    bool yaw_cal();
    bool x_y_cal();
    bool Distance(int oprange);
    bool Anglar(int oprange);
    bool lader_calib_callback(agv_srvs::LidarCalib::Request &req, agv_srvs::LidarCalib::Response &resp);
    bool initPointsData();
    void OdomSubscribeCallback(const agv_msgs::OdometryLite::ConstPtr& msg);
    bool record_x_y();
    bool GetFrontLasertoBaseLinkTf();
    bool set_tf(double x, double y,double theta);


private:
    double pose_x_,pose_x_old;
    double pose_y_,pose_y_old;
    double pose_theta_,pose_theta_old;
    double vel = 0;
    double diff,diff_threshold = 1;
    bool isInversion;
    bool isEnable;
    bool record_trigger = false;
    double rotating_angle;
    bool calib_success_flag,yaw_calib_success_flag = false,x_y_calib_success_flag = false,y_calib_success_flag =false;
    int trigger;
    int node0,node1,node2,node3,node4,node5,fromNode,toNode;
    int percent = -1;
    int currNavType = 0;
    int agvModeOld = 0,agvMode = 0;
    int landmark_tag = 0;
    bool moveDir = 0,node1_0_called = false,node2_0_called = false,node3_0_called = false,node4_0_called = false,node5_0_called = false,node5_1_called = false,node4_1_called = false,node3_1_called = false,node2_1_called = false,node1_1_called = false;
    int moveCycle = 10;
    double cur_theta = 0;
    double landmark_theta = 0;
    //double oprange = 90;
    double delta_threshold = 0.03;

    double pose_x_sum = 0;
    double pose_y_sum = 0;
    double pose_theta_sum = 0;
    double E_pose_x_1,E_pose_y_1,E_pose_theta_1,Var_pose_x_1,Var_pose_y_1,Var_pose_theta_1;
    double E_pose_x_2,E_pose_y_2,E_pose_theta_2,Var_pose_x_2,Var_pose_y_2,Var_pose_theta_2;
    double bias_x_max_1 = 0,bias_y_max_1 = 0,bias_theta_max_1 = 0;
    double bias_x_max_2 = 0,bias_y_max_2 = 0,bias_theta_max_2 = 0;
    double v = 0.08;
    double a_t = M_PI/1440;

    ofstream file1,file2;


    int64_t order_num = 0;
    string lidar_msg;
    vector<std::string> topics;
    vector<double> fixed_x,fixed_y;
    ros::NodeHandle nh_;
    ros::Subscriber pose_sub,moveInfoSub,modeSub,landmarkSub;
    ros::Publisher per_pub;
    ros::ServiceServer server;
    ros::ServiceClient order_client;
    ros::ServiceClient laser_tf_client;
    ros::ServiceClient mode_clinet;
    geometry_msgs::Pose node0_pose;
    geometry_msgs::Pose cur_pose;
    geometry_msgs::Pose node2_pose_record_1,node2_pose_record_2,node3_pose_record_1,node3_pose_record_2,node4_pose_record_1,node4_pose_record_2;
    geometry_msgs::Twist current_vel;
    geometry_msgs::Pose current_odom_pose;
    geometry_msgs::Pose start_pose;
    geometry_msgs::Pose landmark_pose;
    geometry_msgs::Twist vel_msg;
    ros::Subscriber odometry_sub;    /**< Current velocity from odometry */
    ros::Publisher  vel_pub;         /**< Outgoing velocity commands */
    geometry_msgs::Pose odom_pose_array[4];
    geometry_msgs::Pose scan_pose_array[4];

    map_stream_reader map_stream_reader_;
    tf::StampedTransform tfGeom;
    std::vector<::ros::WallTimer> wall_timers_;
    std::vector<PoseRecord> pose_list_1,pose_list_2;
    std::vector<double> bias_x_list_1,bias_y_list_1,bias_th_list_1;
    std::vector<double> bias_x_list_2,bias_y_list_2,bias_th_list_2;
    boost::shared_ptr<boost::thread> work_thread;
};

bool AutoCalib::init()
{
    start_pose.position.x = 0;
    start_pose.position.y = 0;

    nh_.param("/lidar_guidance/lidar_info/lidar1/laser_pose_x",pose_x_,0.0);
    nh_.param("/lidar_guidance/lidar_info/lidar1/laser_pose_y",pose_y_,0.0);
    nh_.param("/lidar_guidance/lidar_info/lidar1/laser_pose_theta",pose_theta_,0.0);
    nh_.param("/lidar_guidance/lidar_info/lidar1/isInversion",isInversion,false);
    nh_.param("/lidar_guidance/lidar_info/lidar1/enable",isEnable,true);
    nh_.param("/lidar_calibration/angel_vel",vel,0.1);
    pose_x_old = pose_x_;
    pose_y_old = pose_y_;
    pose_theta_old = pose_theta_;

    per_pub = nh_.advertise<std_msgs::Int32>("/lidarCalib_percent", 1);
    pose_sub = nh_.subscribe("/pose",1, &AutoCalib::pose_callback, this);
    landmarkSub = nh_.subscribe("/landmark",1,&AutoCalib::landmark_callback, this);
    moveInfoSub = nh_.subscribe("/auto_run_status", 1, &AutoCalib::HandleMoveInfoMessage,this);
    modeSub = nh_.subscribe("/agvmode", 1, &AutoCalib::HandleModeInfoMessage,this);
    odometry_sub =nh_.subscribe("/base/controllers/twist_controller/odom", 1, &AutoCalib::OdomSubscribeCallback,this);
    vel_pub = nh_.advertise<geometry_msgs::Twist>("/base/twist_mux/keyboard_vel",10);
    server = nh_.advertiseService("/lidarCalibration_service",  &AutoCalib::lader_calib_callback, this);
    wall_timers_.push_back(nh_.createWallTimer(::ros::WallDuration(0.3),&AutoCalib::InitMap,this));
    wall_timers_.push_back(nh_.createWallTimer(::ros::WallDuration(1),&AutoCalib::PublishPercent,this));

    wall_timers_.push_back(nh_.createWallTimer(::ros::WallDuration(0.2),&AutoCalib::RecordPose,this));
    GetFrontLasertoBaseLinkTf();
    ros::service::waitForService("/local_order");
    ros::service::waitForService("/setLaserCalib");
    ros::service::waitForService("/operation_mode");

    order_client = nh_.serviceClient<agv_srvs::V_localOrder>("/local_order");
    laser_tf_client = nh_.serviceClient<agv_srvs::SetLaserCalib>("/setLaserCalib");
    mode_clinet = nh_.serviceClient<agv_srvs::SetOperationMode>("/operation_mode");

     if(!initPointsData()){
        ROS_ERROR_STREAM("load xml failed ....");
        return false;
    };
    file1.open("/home/vensin/1.txt");
    file2.open("/home/vensin/2.txt");
    work_thread.reset(new boost::thread(&AutoCalib::work_cycle, this));
    return true;
}

bool AutoCalib::GetFrontLasertoBaseLinkTf(){
    tf::TransformListener tf_Listener;
    try
    {
        tf_Listener.waitForTransform("base_link", "laser_link_1",ros::Time(),ros::Duration(5.0));
        tf_Listener.lookupTransform("base_link", "laser_link_1", ros::Time(0), tfGeom);
    }
    catch (...)
    {
        ROS_ERROR_STREAM("can not find the tf from laser link to base link ! ");
        percent = -2;
        //lidar_msg = "can not find the tf from laser link to base link ! ";
        return false;
    }
    return true;

}

void AutoCalib::HandleMoveInfoMessage(const agv_msgs::AgvAutoStatus::ConstPtr& msg) //agv_msgs::AgvAutoStatus
{
  fromNode = msg->fromNode;
  toNode = msg->toNode;
}

void AutoCalib::HandleModeInfoMessage(const agv_msgs::AGVMode::ConstPtr& msg)
{
  agvMode = msg->mode;
}
void AutoCalib::InitMap(const ros::WallTimerEvent &timer_event)
{
  if(agvMode == 3 && agvModeOld == 0){
    initPointsData();
  }
  agvModeOld = agvMode;
}

void AutoCalib::landmark_callback(const geometry_msgs::PoseStamped &mark_data)
{
    landmark_tag = mark_data.pose.position.z;
    landmark_pose = mark_data.pose;
    landmark_theta = tf::getYaw(mark_data.pose.orientation);
    //ROS_INFO_STREAM("landmark x"<<landmark_pose.position.x<<" landmark y "<<landmark_pose.position.y<<" theta "<< landmark_theta);
}

void AutoCalib::RecordPose(const ros::WallTimerEvent &timer_event)
{
    if(trigger == 4){
        //ROS_INFO_STREAM("current_vel.angular.z "<<current_vel.angular.z<<" current_vel.linear.x "<<current_vel.linear.x<<" current_vel.linear.y "<< current_vel.linear.y
        //<< "from node "<<fromNode<<" toNode "<<toNode <<" record_trigger "<< record_trigger);
        if (fromNode == node4 && toNode == node4 && record_trigger == false){
            //ROS_ERROR_STREAM("11111111111111111111111111111111111111111111111111");
            if(abs(current_vel.angular.z)<0.0001 && abs(current_vel.linear.x) < 0.0001 && abs(current_vel.linear.y)<0.0001){
                //ROS_ERROR_STREAM("22222222222222222222222222222222222222222222222222222222");
                PoseRecord pose;
                pose.x_ = landmark_pose.position.x;
                pose.y_ = landmark_pose.position.y;
                pose.theta_ = landmark_theta;
                if(pose.theta_ < 0){
                    pose.theta_ = pose.theta_ + 2*M_PI;
                }
                pose_list_1.push_back(pose);
                record_trigger = true;
                pose_x_sum = pose_x_sum + pose.x_;
                pose_y_sum = pose_y_sum + pose.y_;
                pose_theta_sum = pose_theta_sum + pose.theta_;
                ROS_ERROR_STREAM("pose.x_ "<<pose.x_<<" pose.y_ "<<pose.y_<<" theta "<< pose.theta_);
                file1 << pose.x_ << " " << pose.y_<< " " << pose.theta_ << std::endl;
            }
        }
        else if (fromNode == node5 && toNode == node5 && record_trigger == false){
            //ROS_ERROR_STREAM("11111111111111111111111111111111111111111111111111");
            if(abs(current_vel.angular.z)<0.0001 && abs(current_vel.linear.x) < 0.0001 && abs(current_vel.linear.y)<0.0001){
                //ROS_ERROR_STREAM("22222222222222222222222222222222222222222222222222222222");
                PoseRecord pose;
                pose.x_ = landmark_pose.position.x;
                pose.y_ = landmark_pose.position.y;
                pose.theta_ = landmark_theta;
                 if(pose.theta_ < 0){
                    pose.theta_ = pose.theta_ + 2*M_PI;
                }
                pose_list_2.push_back(pose);
                record_trigger = true;
                pose_x_sum = pose_x_sum + pose.x_;
                pose_y_sum = pose_y_sum + pose.y_;
                pose_theta_sum = pose_theta_sum + pose.theta_;
                ROS_ERROR_STREAM("pose.x_ "<<pose.x_<<" pose.y_ "<<pose.y_<<" theta "<< pose.theta_);
                file2 << pose.x_ << " " << pose.y_<< " " << pose.theta_ << std::endl;
            }
        }
        else if((fromNode == node5 && toNode != node5) || (fromNode == node4 && toNode != node4))
        {
            record_trigger = false;
        }

    }
    else{
        pose_list_1.clear();
        pose_list_2.clear();
        pose_x_sum = 0;
        pose_y_sum = 0;
        pose_theta_sum = 0;
        E_pose_x_1 = 0;
        E_pose_y_1 = 0;
        E_pose_theta_1 = 0;

        E_pose_x_2 = 0;
        E_pose_y_2 = 0;
        E_pose_theta_2 = 0;

        // Var_pose_x = 0;
        // Var_pose_y = 0;
        // Var_pose_theta = 0;
        bias_x_max_1 = 0;
        bias_y_max_1 = 0;
        bias_theta_max_1 = 0;

        bias_x_max_2 = 0;
        bias_y_max_2 = 0;
        bias_theta_max_2 = 0;
    }
}


void AutoCalib::PublishPercent(const ros::WallTimerEvent &timer_event)
{
    std_msgs::Int32 per;
    per.data = percent;
    per_pub.publish(per);
    //ROS_ERROR_STREAM("trigger is "<< trigger);
    E_pose_x_1 = pose_x_sum/pose_list_1.size();
    E_pose_y_1 = pose_y_sum/pose_list_1.size();
    E_pose_theta_1 = pose_theta_sum/pose_list_1.size();
    double x_temp = 0, y_temp = 0, theta_temp = 0;
    for(const auto pose : pose_list_1){
        //x_temp = x_temp + pow(pose.x_ - E_pose_x,2);
        //if(pose.x_ - E_pose_x_1>bias_x_max)bias_x_max = pose.x_ - E_pose_x;
        bias_x_list_1.push_back(pose.x_ - E_pose_x_1);
        //y_temp = y_temp + pow(pose.y_ - E_pose_y,2);
        //if(pose.y_ - E_pose_y_1 > bias_y_max)bias_y_max = pose.y_ - E_pose_y;
        bias_y_list_1.push_back(pose.x_ - E_pose_x_1);
        //theta_temp = theta_temp + pow(pose.theta_ - E_pose_theta,2);
        //if(pose.theta_ - E_pose_theta_1>bias_theta_max)bias_theta_max = pose.theta_ - E_pose_theta;
        bias_th_list_1.push_back(pose.theta_ - E_pose_theta_1);
    }
    // Var_pose_x = x_temp/pose_list.size();
    // Var_pose_y = y_temp/pose_list.size();
    // Var_pose_theta = theta_temp/pose_list.size();
    for(const auto pose : pose_list_2){
        //x_temp = x_temp + pow(pose.x_ - E_pose_x,2);
        //if(pose.x_ - E_pose_x_1>bias_x_max)bias_x_max = pose.x_ - E_pose_x;
        bias_x_list_2.push_back(pose.x_ - E_pose_x_2);
        //y_temp = y_temp + pow(pose.y_ - E_pose_y,2);
        //if(pose.y_ - E_pose_y_1 > bias_y_max)bias_y_max = pose.y_ - E_pose_y;
        bias_y_list_2.push_back(pose.x_ - E_pose_x_2);
        //theta_temp = theta_temp + pow(pose.theta_ - E_pose_theta,2);
        //if(pose.theta_ - E_pose_theta_1>bias_theta_max)bias_theta_max = pose.theta_ - E_pose_theta;
        bias_th_list_2.push_back(pose.theta_ - E_pose_theta_2);
    }
    // ROS_WARN_STREAM(
    //     "Ex "<< E_pose_x_1 << " Ey "<<E_pose_y_1<<" Eth "<< E_pose_theta_1
    //     //<<  " Var x " << Var_pose_x << " Var y "<< Var_pose_y << " var th "<< Var_pose_theta
    //     // << " bias x max "<< bias_x_max <<  " bias y max "<< bias_y_max << " bias th max "<< bias_theta_max
    // );
}



bool AutoCalib::initPointsData()
{
    if(!map_stream_reader_.load_map_xmlfile()){
        ROS_ERROR_STREAM("can not load points!");
        return false;
    }
    return true;
}

void AutoCalib::pose_callback(const::agv_msgs::PoseWithConfidence& msg)
{
    cur_pose = msg.pose.pose;
    cur_theta = tf::getYaw(msg.pose.pose.orientation);
}

bool AutoCalib::lader_calib_callback(agv_srvs::LidarCalib::Request &req, agv_srvs::LidarCalib::Response &resp)
{
    trigger = req.trigger;

    node0 = req.node0;
    node1 = req.node1;
    node2 = req.node2;
    node3 = req.node3;
    node4 = req.node4;
    node5 = req.node5;

    // node0 = 1259;
    // node1 = 1260;
    // node2 = 1262;
    // node3 = 1263;
    // node4 = 1264;
    // node5 = 1265;

    node0_pose.position.x = map_stream_reader_.pointsMap[node0].x;
    node0_pose.position.y = map_stream_reader_.pointsMap[node0].y;
    diff = getdistance(node0_pose,cur_pose);
    ROS_INFO("call server-----------------trigger:%d", trigger);
    if(trigger==1) {
        if(diff > diff_threshold){
            lidar_msg = "please move agv to node 0";
            resp.msg = lidar_msg;
            resp.success = 2;
        }
        else if(percent >0 && percent <100){
            lidar_msg = "calibrating";
            resp.msg = lidar_msg;
            resp.success = 0;
        }
        else{
            agv_srvs::SetOperationMode::Request req;
            agv_srvs::SetOperationMode::Response ret;
            //ros::service::waitForService("/operation_mode");
            req.mode = "auto";
            req.sender = 1;
            req.submode = "local";
            mode_clinet.call(req,ret);
            std::this_thread::sleep_for(std::chrono::seconds(1));
            lidar_msg = "start";
            percent = 25;
            moveCycle = 10;
            resp.msg = lidar_msg;
            resp.success = 1;
            calib_success_flag = false;
            yaw_calib_success_flag = false;
            x_y_calib_success_flag = false;
            y_calib_success_flag = false;
            fixed_x.clear();
            fixed_y.clear();
        }

    }
    else if(trigger == 2) {
        lidar_msg = "cancel";
        percent = 0;
        resp.msg = lidar_msg;
        resp.success = 1;
        calib_success_flag = false;
        yaw_calib_success_flag = false;
        x_y_calib_success_flag = false;
        set_tf(pose_x_old,pose_y_old,pose_theta_old);
        moveDir = 0;
        node1_0_called = false;
        node2_0_called = false;
        node3_0_called = false;
        node4_0_called = false;
        node5_0_called = false;
        node5_1_called = false;
        node4_1_called = false;
        node3_1_called = false;
        node2_1_called = false;
        node1_1_called = false;
    }
    else if(trigger == 3) {
        if(calib_success_flag == false && percent <100 && percent >0){
            lidar_msg = "calibrating ...";
            resp.msg = lidar_msg;
            resp.success = 0;

        }
        else if(calib_success_flag == true){
            lidar_msg = "calibration finish!";
            resp.x = pose_x_;
            resp.y = pose_y_;
            resp.angle = pose_theta_;
            resp.msg = lidar_msg;
            resp.success = 1;
        }
        else if(calib_success_flag == false && percent == 0){
            lidar_msg = "calibration not start!";
            resp.msg = lidar_msg;
            resp.success = 2;
        }
        else if(percent < 0){
            lidar_msg = "calibration failed!";
            resp.msg = lidar_msg;
            resp.success = 2;
        }
    }
    else if(trigger == 4){

    }
    else{
        resp.msg = "request error ";
    }
    return true;
}

bool AutoCalib::yaw_cal()
{
    double delta_x,delta_y,delta_yaw;
    if(map_stream_reader_.pointsMap[node2].x == map_stream_reader_.pointsMap[node5].x){
        delta_x = node4_pose_record_1.position.x - map_stream_reader_.pointsMap[node4].x;
        delta_y = node4_pose_record_1.position.y - node2_pose_record_1.position.y;
        ROS_ERROR_STREAM("delta x "<< delta_x << "delta y "<<delta_y);
        delta_yaw = atan2(delta_x,abs(delta_y));
        // if(map_stream_reader_.pointsMap[node2].y > map_stream_reader_.pointsMap[node5].y){
        //     delta_yaw = -delta_yaw;
        // }
        if(abs(cur_theta - M_PI_2)<10*M_PI/180){
            delta_yaw = delta_yaw;
        }
        else if(abs(cur_theta + M_PI_2)<10*M_PI/180){
            delta_yaw = -delta_yaw;
        }

    }
    else if(map_stream_reader_.pointsMap[node2].y == map_stream_reader_.pointsMap[node5].y){
        delta_x = node4_pose_record_1.position.x - node2_pose_record_1.position.x;
        delta_y = node4_pose_record_1.position.y - map_stream_reader_.pointsMap[node4].y;
        delta_yaw = atan2(delta_y,abs(delta_x));
        ROS_ERROR_STREAM("delta x "<< delta_x << "delta y "<<delta_y);
        if(abs(cur_theta - M_PI)<=10*M_PI/180 || abs(cur_theta + M_PI)<=10*M_PI/180){
            delta_yaw = delta_yaw;
        }
        else if(abs(cur_theta - 0)<10*M_PI/180){
        //else if(map_stream_reader_.pointsMap[node2].x < map_stream_reader_.pointsMap[node5].x){
            delta_yaw = -delta_yaw;
        }
    }
    ROS_ERROR_STREAM("delta_yaw is "<< delta_yaw);
    if(isInversion == false){
        pose_theta_ = pose_theta_ + delta_yaw;
    }
    else if(isInversion == true){
        pose_theta_ = pose_theta_ - delta_yaw;
    }
    ROS_ERROR_STREAM("pose x "<< pose_x_ << " pose y "<<pose_y_<<" pose_theta_ "<< pose_theta_);
    set_tf(pose_x_,pose_y_,pose_theta_);
    if(abs(delta_yaw) < M_PI/a_t){ //0.5°
        yaw_calib_success_flag = true;
        percent = 50;
        moveDir = 1;
        node1_1_called = false;
    }
    //ROS_INFO_STREAM("66666666666666666666666666666666666666666666666666666666666666666");
    return true;
}


bool AutoCalib::x_y_cal(){
    ROS_WARN_STREAM("x y caculating!!!!!");
    bool ret = false;
    vel = 0.1;
    scan_pose_array[0] = cur_pose;
    odom_pose_array[0] = current_odom_pose;
    //ROS_INFO_STREAM("111111111111111111111111111111111111111111111");
    if(trigger == 2)return false;
    Anglar(90);
    std::this_thread::sleep_for(std::chrono::seconds(3));
    scan_pose_array[1] = cur_pose;
    odom_pose_array[1] = current_odom_pose;
    //ROS_INFO_STREAM("2222222222222222222222222222222222222222222222");
    if(trigger == 2)return false;
    Anglar(90);
    std::this_thread::sleep_for(std::chrono::seconds(3));
    scan_pose_array[2] = cur_pose;
    odom_pose_array[2] = current_odom_pose;
    //ROS_INFO_STREAM("3333333333333333333333333333333333333333333333");
    if(trigger == 2)return false;
    Anglar(90);
    std::this_thread::sleep_for(std::chrono::seconds(3));
    scan_pose_array[3] = cur_pose;
    odom_pose_array[3] = current_odom_pose;
    //ROS_INFO_STREAM("4444444444444444444444444444444444444444444444");
    if(trigger == 2)return false;
    Anglar(90);
    ret = record_x_y();
    if(ret == true){
        return ret;
    }
    std::this_thread::sleep_for(std::chrono::seconds(3));
    vel = -0.1;
    scan_pose_array[0] = cur_pose;
    odom_pose_array[0] = current_odom_pose;
    //ROS_INFO_STREAM("5555555555555555555555555555555555555555555555");
    //Anglar(-90);
    if(trigger == 2)return false;
    Anglar(90);
    std::this_thread::sleep_for(std::chrono::seconds(3));
    scan_pose_array[1] = cur_pose;
    odom_pose_array[1] = current_odom_pose;
    //ROS_INFO_STREAM("666666666666666666666666666666666666666666666");
    //Anglar(-90);
    if(trigger == 2)return false;
    Anglar(90);
    std::this_thread::sleep_for(std::chrono::seconds(3));
    scan_pose_array[2] = cur_pose;
    odom_pose_array[2] = current_odom_pose;
    //ROS_INFO_STREAM("77777777777777777777777777777777777777777777");
    //Anglar(-90);
    if(trigger == 2)return false;
    Anglar(90);
    std::this_thread::sleep_for(std::chrono::seconds(3));
    scan_pose_array[3] = cur_pose;
    odom_pose_array[3] = current_odom_pose;
    ret = record_x_y();
    if(ret == true){
        return ret;
    }
    pose_x_ = pose_x_ + (std::accumulate(fixed_x.begin(), fixed_x.end(), 0.)/fixed_x.size())/2;
    pose_y_ = pose_y_ + (std::accumulate(fixed_y.begin(), fixed_y.end(), 0.)/fixed_y.size())/2;
    //ROS_INFO_STREAM("88888888888888888888888888888888888888888888");
    //ROS_WARN_STREAM("pose_x_ "<<pose_x_<<" pose_y_ "<<pose_y_);
    //Anglar(-90);
    if(trigger == 2)return false;
    Anglar(90);
    std::this_thread::sleep_for(std::chrono::seconds(3));
    ROS_ERROR_STREAM("pose x "<< pose_x_ << " pose y "<<pose_y_<<" pose_theta_ "<< pose_theta_<<" for x y cal!");
    set_tf(pose_x_,pose_y_,pose_theta_);
    fixed_x.clear();
    fixed_y.clear();
    std::this_thread::sleep_for(std::chrono::seconds(3));
    return false;

}

bool AutoCalib::record_x_y(){
    ROS_ERROR_STREAM("recording !!!!!!!!!!!!!!!");
    bool x_y_calib_success_flag_1 = false;
    double fixed_delta_y,fixed_delta_x;
    if(abs(tf::getYaw(scan_pose_array[0].orientation))<=30*M_PI/180){
        fixed_delta_x = scan_pose_array[0].position.x - scan_pose_array[2].position.x - (odom_pose_array[0].position.x - odom_pose_array[2].position.x);
        fixed_delta_y = scan_pose_array[0].position.y - scan_pose_array[2].position.y - (odom_pose_array[0].position.y - odom_pose_array[2].position.y);
        fixed_x.push_back(fixed_delta_x);
        fixed_y.push_back(fixed_delta_y);
        ROS_WARN_STREAM("fixed_delta_x "<<fixed_delta_x<<" fixed_delta_y "<<fixed_delta_y);
        if(abs(fixed_delta_x)<delta_threshold && abs(fixed_delta_y)<delta_threshold){
            x_y_calib_success_flag_1 = true;
            //percent = 100;
            //return true;
        }
        else{x_y_calib_success_flag_1 = false;}
        if(vel>0){
            fixed_delta_y = scan_pose_array[1].position.x - scan_pose_array[3].position.x - (odom_pose_array[1].position.x - odom_pose_array[3].position.x);
            fixed_delta_x = scan_pose_array[3].position.y - scan_pose_array[1].position.y - (odom_pose_array[3].position.y - odom_pose_array[1].position.y);
            fixed_x.push_back(fixed_delta_x);
            fixed_y.push_back(fixed_delta_y);
        }
        else{
            fixed_delta_y = scan_pose_array[3].position.x - scan_pose_array[1].position.x - (odom_pose_array[3].position.x - odom_pose_array[1].position.x);
            fixed_delta_x = scan_pose_array[1].position.y - scan_pose_array[3].position.y - (odom_pose_array[1].position.y - odom_pose_array[3].position.y);
            fixed_x.push_back(fixed_delta_x);
            fixed_y.push_back(fixed_delta_y);
        }
        ROS_WARN_STREAM("fixed_delta_x "<<fixed_delta_x<<" fixed_delta_y "<<fixed_delta_y);
        if(abs(fixed_delta_x)<delta_threshold && abs(fixed_delta_y)<delta_threshold && x_y_calib_success_flag_1 == true){
            x_y_calib_success_flag = true;
            percent = 100;
            return true;
        }
    }
    else if(abs(tf::getYaw(scan_pose_array[0].orientation)-M_PI)<=30*M_PI/180  || (tf::getYaw(scan_pose_array[0].orientation)+M_PI)<=30*M_PI/180){
        fixed_delta_x = scan_pose_array[2].position.x - scan_pose_array[0].position.x - (odom_pose_array[2].position.x - odom_pose_array[0].position.x);
        fixed_delta_y = scan_pose_array[2].position.y - scan_pose_array[0].position.y - (odom_pose_array[2].position.y - odom_pose_array[0].position.y);
        fixed_x.push_back(fixed_delta_x);
        fixed_y.push_back(fixed_delta_y);
        ROS_WARN_STREAM("fixed_delta_x "<<fixed_delta_x<<" fixed_delta_y "<<fixed_delta_y);
        if(abs(fixed_delta_x)<delta_threshold && abs(fixed_delta_y)<delta_threshold){
            x_y_calib_success_flag_1 = true;
            // percent = 100;
            // return true;
        }
        if(vel>0){
            fixed_delta_x = scan_pose_array[3].position.y - scan_pose_array[1].position.y - (odom_pose_array[3].position.y - odom_pose_array[1].position.y);
            fixed_delta_y = scan_pose_array[1].position.x - scan_pose_array[3].position.x - (odom_pose_array[1].position.x - odom_pose_array[3].position.x);
            fixed_x.push_back(fixed_delta_x);
            fixed_y.push_back(fixed_delta_y);
        }
        else{
            fixed_delta_x = scan_pose_array[1].position.y - scan_pose_array[3].position.y - (odom_pose_array[1].position.y - odom_pose_array[3].position.y);
            fixed_delta_y = scan_pose_array[3].position.x - scan_pose_array[1].position.x - (odom_pose_array[3].position.x - odom_pose_array[1].position.x);
            fixed_x.push_back(fixed_delta_x);
            fixed_y.push_back(fixed_delta_y);
        }
        ROS_WARN_STREAM("fixed_delta_x "<<fixed_delta_x<<" fixed_delta_y "<<fixed_delta_y);
        if(abs(fixed_delta_x)<delta_threshold && abs(fixed_delta_y)<delta_threshold && x_y_calib_success_flag_1 == true){
            x_y_calib_success_flag = true;
            percent = 100;
            return true;
        }
    }

    else if(abs(tf::getYaw(scan_pose_array[0].orientation)-M_PI_2)<=30*M_PI/180){
        fixed_delta_y = scan_pose_array[2].position.x - scan_pose_array[0].position.x - (odom_pose_array[2].position.x - odom_pose_array[0].position.x);
        fixed_delta_x = scan_pose_array[0].position.y - scan_pose_array[2].position.y - (odom_pose_array[0].position.y - odom_pose_array[2].position.y);
        fixed_x.push_back(fixed_delta_x);
        fixed_y.push_back(fixed_delta_y);
        ROS_WARN_STREAM("fixed_delta_x "<<fixed_delta_x<<" fixed_delta_y "<<fixed_delta_y);
        if(abs(fixed_delta_x)<delta_threshold && abs(fixed_delta_y)<delta_threshold){
            x_y_calib_success_flag_1 = true;
            // percent = 100;
            // return true;
        }
        if(vel>0){
            fixed_delta_y = scan_pose_array[3].position.y - scan_pose_array[1].position.y - (odom_pose_array[3].position.y - odom_pose_array[1].position.y);
            fixed_delta_x = scan_pose_array[3].position.x - scan_pose_array[1].position.x - (odom_pose_array[3].position.x - odom_pose_array[1].position.x);
            fixed_x.push_back(fixed_delta_x);
            fixed_y.push_back(fixed_delta_y);
        }
        else{
            fixed_delta_y = scan_pose_array[1].position.y - scan_pose_array[3].position.y - (odom_pose_array[1].position.y - odom_pose_array[3].position.y);
            fixed_delta_x = scan_pose_array[1].position.x - scan_pose_array[3].position.x - (odom_pose_array[1].position.x - odom_pose_array[3].position.x);
            fixed_x.push_back(fixed_delta_x);
            fixed_y.push_back(fixed_delta_y);
        }
        ROS_WARN_STREAM("fixed_delta_x "<<fixed_delta_x<<" fixed_delta_y "<<fixed_delta_y);
        if(abs(fixed_delta_x)<delta_threshold && abs(fixed_delta_y)<delta_threshold && x_y_calib_success_flag_1 == true){
            x_y_calib_success_flag = true;
            percent = 100;
            return true;
        }
    }

    else if(abs(tf::getYaw(scan_pose_array[0].orientation)+M_PI_2)<=30*M_PI/180){
        fixed_delta_y = scan_pose_array[0].position.x - scan_pose_array[2].position.x - (odom_pose_array[0].position.x - odom_pose_array[2].position.x);
        fixed_delta_x = scan_pose_array[2].position.y - scan_pose_array[0].position.y - (odom_pose_array[2].position.y - odom_pose_array[0].position.y);
        fixed_x.push_back(fixed_delta_x);
        fixed_y.push_back(fixed_delta_y);
        ROS_WARN_STREAM("fixed_delta_x "<<fixed_delta_x<<" fixed_delta_y "<<fixed_delta_y);
        if(abs(fixed_delta_x)<delta_threshold && abs(fixed_delta_y)<delta_threshold){
            x_y_calib_success_flag_1 = true;
            // percent = 100;
            // return true;
        }
        if(vel>0){
            fixed_delta_y = scan_pose_array[1].position.y - scan_pose_array[3].position.y - abs(odom_pose_array[1].position.y - odom_pose_array[3].position.y);
            fixed_delta_x = scan_pose_array[1].position.x - scan_pose_array[3].position.x - abs(odom_pose_array[1].position.x - odom_pose_array[3].position.x);
            fixed_x.push_back(fixed_delta_x);
            fixed_y.push_back(fixed_delta_y);
        }
        else{
            fixed_delta_y = scan_pose_array[3].position.y - scan_pose_array[1].position.y - (odom_pose_array[3].position.y - odom_pose_array[1].position.y);
            fixed_delta_x = scan_pose_array[3].position.x - scan_pose_array[1].position.x - (odom_pose_array[3].position.x - odom_pose_array[1].position.x);
            fixed_x.push_back(fixed_delta_x);
            fixed_y.push_back(fixed_delta_y);
        }
        ROS_WARN_STREAM("fixed_delta_x "<<fixed_delta_x<<" fixed_delta_y "<<fixed_delta_y);
        if(abs(fixed_delta_x)<delta_threshold && abs(fixed_delta_y)<delta_threshold && x_y_calib_success_flag_1 == true){
            x_y_calib_success_flag = true;
            percent = 100;
            return true;
        }
    }
    return false;
}

bool AutoCalib::line_move(int fromNode,int toNode)
{
    ROS_INFO_STREAM("line_move-----------------distance");
    //ros::Rate rate(10);
    agv_msgs::V_order order;
    order.orderId = std::to_string(order_num);
    //order.orderUpdateId = order_num;
    agv_msgs::V_node node;
    agv_srvs::V_localOrder::Request req;
    agv_srvs::V_localOrder::Response res;
    node.nodeId = std::to_string(fromNode);
    order.nodes.push_back(node);
    node.nodeId = std::to_string(toNode);
    order.nodes.push_back(node);
    req.order = order;
    req.recycle = false;
    order_client.call(req,res);
    order_num ++;
    ROS_WARN_STREAM(res.message);
    if(res.success == true)return true;
    else return false;
}

bool AutoCalib::set_tf(double x, double y,double theta)
{
    agv_srvs::SetLaserCalib::Request req;
    agv_srvs::SetLaserCalib::Response res;
    req.laser_num = 1;
    req.enable.push_back(isEnable);
    req.isInversion.push_back(isInversion);
    req.x.push_back(x);
    req.y.push_back(y);
    req.yaw.push_back(theta);
    try{
        laser_tf_client.call(req,res);
        ROS_INFO_STREAM(res);
    }
    catch(...){
        ROS_ERROR_STREAM("set lidar tf failed!!!!");
    }
    return true;
}


void AutoCalib::work_cycle()
{
    //ROS_INFO_STREAM("@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@");
    ros::Rate rate1(1);
    ros::Rate rate2(0.5);
    while(ros::ok())
    {
        //ROS_WARN_STREAM("trigger is "<< &trigger);
        if(trigger == 2){
                ROS_WARN_STREAM("Calibration Interrupted!");
                rate1.sleep();
                continue;
        }

        if(yaw_calib_success_flag == true && x_y_calib_success_flag == true){
                ROS_INFO_STREAM("Calibration finished!");
                rate1.sleep();
                continue;
        }
        if(percent < 0){
                ROS_INFO_STREAM("Calibration failed!");
                rate1.sleep();
                continue;
        }

        if(trigger == 1){
            ROS_WARN_STREAM("diff is "<< diff);
            bool move_ret,cal_ret;
            if (diff < diff_threshold){
                move_ret = line_move(node0,node1);
                moveDir = 0;
            }
            else{
                ROS_WARN_STREAM("please check map id and move agv to start node!");
                rate1.sleep();
                continue;
            }

            while(moveCycle >= 0 && trigger != 2 ){
                if(moveCycle == 0){
                    percent = -1;
                    set_tf(pose_x_old,pose_y_old,pose_theta_old);
                    calib_success_flag = false;
                    ROS_INFO_STREAM("Laser calibration failure ! please check map set and chassis arguments !");
                    break;
                }
                //ROS_INFO_STREAM("moveCycle is "<< moveCycle << " from node "<<fromNode<<" to node "<< toNode << " move dir "<< moveDir << " node1_1_called "<< node1_1_called << " yaw_calib_success_flag "<<yaw_calib_success_flag << "x_y_calib_success_flag" << x_y_calib_success_flag);
                        if(move_ret == true && fromNode == node1 && toNode == node1 && moveDir == 0 && yaw_calib_success_flag == false && trigger != 2 && node1_0_called ==false){
                            //ROS_INFO_STREAM("1111111111111111111111111111111111"<<" from "<<node1<<" to "<<node2);
                            move_ret = line_move(node1,node2);
                            node1_0_called =true;
                        }
                        else if(move_ret == true && fromNode == node2 && toNode == node2 && moveDir == 0 && yaw_calib_success_flag == false && trigger != 2 && node2_0_called ==false){
                            rate2.sleep();
                            node2_pose_record_1 = cur_pose;
                            //ROS_INFO_STREAM("222222222222222222222222222222222222222"<<" from "<<node2<<" to "<<node3);
                            move_ret = line_move(node2,node3);
                            node2_0_called =true;
                        }
                        else if(move_ret == true && fromNode == node3 && toNode == node3 && moveDir == 0 && yaw_calib_success_flag == false && trigger != 2 && node3_0_called == false){
                            rate2.sleep();
                            node3_pose_record_1 = cur_pose;
                            //ROS_INFO_STREAM("33333333333333333333333333333333333333"<<" from "<<node3<<" to "<<node4);
                            move_ret = line_move(node3,node4);
                            node3_0_called = true;
                        }
                        else if(move_ret == true && fromNode == node4 && toNode == node4 && moveDir == 0 && yaw_calib_success_flag == false && trigger != 2 && node4_0_called == false){
                            rate2.sleep();
                            node4_pose_record_1 = cur_pose;
                            //ROS_INFO_STREAM("444444444444444444444444444444444444"<<" from "<<node4<<" to "<<node5);
                            move_ret = line_move(node4,node5);
                            node4_0_called = true;
                        }
                        else if(move_ret == true && fromNode == node5 && toNode == node5 && moveDir == 0 && yaw_calib_success_flag == false && trigger != 2 && node5_0_called == false){
                            rate2.sleep();
                            //ROS_INFO_STREAM("55555555555555555555555555555555555"<<" from "<<node5<<" to "<<node4);
                            move_ret = line_move(node5,node4);
                            moveDir = 1;
                            node5_0_called = true;
                            node4_0_called = false;
                            node3_0_called = false;
                            node2_0_called = false;
                            node1_0_called = false;
                            node1_1_called = false;
                        }
                        else if(move_ret == true && fromNode == node4 && toNode == node4 && moveDir == 1 && yaw_calib_success_flag == false && trigger != 2 && node4_1_called == false){
                            rate2.sleep();
                            node4_pose_record_2 = cur_pose;
                            //ROS_INFO_STREAM("666666666666666666666666666666666666"<< " from "<<node4<<" to "<<node3);
                            move_ret = line_move(node4,node3);
                            node4_1_called = true;
                        }
                        else if(move_ret == true && fromNode == node3 && toNode == node3 && moveDir == 1 && yaw_calib_success_flag == false && trigger != 2 && node3_1_called == false){
                            rate2.sleep();
                            node3_pose_record_2 = cur_pose;
                            //ROS_INFO_STREAM("7777777777777777777777777777777777777"<< " from "<<node3<<" to "<<node2);
                            move_ret = line_move(node3,node2);
                            node3_1_called = true;
                        }
                        else if(move_ret == true && fromNode == node2 && toNode == node2 && moveDir == 1 && yaw_calib_success_flag == false && trigger != 2 && node2_1_called == false){
                            rate2.sleep();
                            node2_pose_record_2 = cur_pose;
                            //ROS_INFO_STREAM("888888888888888888888888888888888888888888"<< " from "<<node2<<" to "<<node1);
                            move_ret = line_move(node2,node1);
                            node2_1_called = true;
                        }
                        else if(move_ret == true && fromNode == node1 && toNode == node1 && moveDir == 1 && trigger != 2 && node1_1_called == false){
                            rate2.sleep();
                            if(yaw_calib_success_flag == false){
                                //ROS_INFO_STREAM("999999999999999999999999999999999999999");
                                moveDir = 0;
                                node1_1_called = true;
                                bool ret = yaw_cal();
                                ROS_INFO_STREAM(" ret is "<< ret);
                            }
                            else if(yaw_calib_success_flag == true && x_y_calib_success_flag == false){
                                //ROS_INFO_STREAM("00000000000000000000000000000000000000");
                                cal_ret = x_y_cal();
                                if(cal_ret == true) break;
                            }
                            node4_1_called = false;
                            node3_1_called = false;
                            node2_1_called = false;
                            node5_0_called = false;

                            rate1.sleep();
                            ROS_INFO_STREAM("moveCycle "<< moveCycle);
                            moveCycle --;

                        }
                        // else if(trigger == 2){
                        //     break;
                        // }
                        rate1.sleep();
                    }

        }
        if(x_y_calib_success_flag == true && yaw_calib_success_flag == true){
            calib_success_flag = true;
            trigger = 0;
        }
        else{
            calib_success_flag = false;
            //trigger = 0;
        }
        rate1.sleep();
    }

}

bool AutoCalib::Distance( int oprange) {
    ros::Rate spin_rate(50);
    int delay = 0, count = 0;
    geometry_msgs::Twist vel_msg;
    vel_msg.linear.x = 0;
    while (trigger!=2 && ros::ok())
    {
        if (yaw_calib_success_flag == true) {

            if (vel_msg.linear.x < vel)
            {
                vel_msg.linear.x = vel_msg.linear.x + 0.02;
            }
            // vel_msg.linear.x = vel;a_t
            vel_msg.angular.z = 0;

            if(start_pose.position.y == 0.0 && start_pose.position.x == 0.0) {

                start_pose.position.y = current_odom_pose.position.y;
                start_pose.position.x = current_odom_pose.position.x;
                ROS_INFO("Start test - dis:%0.2f,  cur pose:[%0.4f, %0.4f], vel:%.2f",
                    oprange, current_odom_pose.position.x, current_odom_pose.position.y, vel);
                delay = 10;

            } else if (fabs(getdistance(start_pose, current_odom_pose)) >= oprange) {

                vel_msg.linear.x = 0;
                vel_msg.angular.z = 0;

                if (fabs(current_vel.linear.x) < 0.0001)
                {
                    if (delay-- <= 0) {
                        ROS_INFO("End test - dis:[x:%0.4f, y:%0.4f]",
                                        fabs(current_odom_pose.position.x - start_pose.position.x),
                                        fabs(current_odom_pose.position.y - start_pose.position.y));

                        start_pose.position.x = 0.0;
                        start_pose.position.y = 0.0;
                        delay = 0;
                        //shutdown_req = true;
                        break;
                    }
                }

            } else {
                ROS_INFO("moving - target:%0.2f, dis:[%0.4f], vel:%.2f",
                    oprange, fabs(getdistance(start_pose, current_odom_pose)), vel_msg.linear.x);
            }

        }
        vel_pub.publish(vel_msg);
        ros::spinOnce();
        spin_rate.sleep();
    }
    return true;
}

bool AutoCalib::Anglar(int oprange) {
    ros::Rate spin_rate(50);
    int delay = 0, count = 0;
    double theta = 0;
    bool init_yaw = false;
    if(trigger == 2)return false;
    while (trigger!=2 && ros::ok())
    {
        geometry_msgs::Twist vel_msg;
		if (yaw_calib_success_flag == true) {

			vel_msg.angular.z = vel;
			vel_msg.linear.x = 0;

			tf::Quaternion quat;
			tf::quaternionMsgToTF(current_odom_pose.orientation, quat);
            //ROS_INFO_STREAM("w"<<current_odom_pose.orientation.w <<"z"<< current_odom_pose.orientation.z);
  			double roll, pitch, yaw;
            tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);

			if (!init_yaw) {

				init_yaw = true;
				count = 0;
				//theta = yaw + (vel > 0 ? oprange : oprange * -1) * M_PI / 180.0;
                theta = yaw + (vel > 0 ? oprange : oprange * -1) * M_PI / 180.0;

				if(theta >= M_PI) theta = theta - 2 * M_PI;
				if(theta <= -M_PI) theta = theta + 2 * M_PI;
				//ROS_INFO("Start test - curret ang:[%0.2f], target ang:[%0.2f], ang vel:%.2f", yaw * 180.0 / M_PI, theta * 180.0 / M_PI, vel);

			} else {

				if (count > 100 && fabs(yaw - theta) <= 0.02) {
					vel_msg.linear.x = 0;
					vel_msg.angular.z = 0;

                    if (fabs(current_vel.angular.z) < 0.0001) {
                        if (delay-- <= 0) {
                            //ROS_INFO("End test - stop ang:[%0.2f], target ang:[%0.2f], diff:%.2f", yaw * 180.0 / M_PI, theta * 180.0 / M_PI, fabs(yaw - theta));
                            theta = 0;
                            delay = 0;
                            init_yaw = false;
                            //shutdown_req = true;
                            break;
                        }
                    }

                } else {
                    count++;
                    //ROS_INFO("Rotating - ang:[%0.2f], diff:[%0.2f], ang vel:[%0.2f]", yaw * 180.0 / M_PI, fabs(yaw - theta), current_vel.angular.z * 180.0 / M_PI);
                }
			}

		}
        vel_pub.publish(vel_msg);
        ros::spinOnce();
        spin_rate.sleep();
    }
    return true;
}
void AutoCalib::OdomSubscribeCallback(const agv_msgs::OdometryLite::ConstPtr& msg)
{
	current_odom_pose.position.x = msg->pose.position.x;
	current_odom_pose.position.y = msg->pose.position.y;
	current_odom_pose.orientation = msg->pose.orientation;
	current_vel = msg->twist;
    //ROS_INFO_STREAM("w"<<current_odom_pose.orientation.w <<"z"<< current_odom_pose.orientation.z);
}


int main(int argc, char** argv) {
    setlocale(LC_ALL,"");
    ros::init(argc, argv, "testcalib");
    ros::NodeHandle nh("~");
    ros::AsyncSpinner spinner(4);
    AutoCalib calibNode_(nh);
    calibNode_.init();
    spinner.start();
    ros::Rate rate(1);
    while(ros::ok()) {
        rate.sleep();
        ros::spinOnce();
    }
    return 0;
}

